"""《RRT算法》
    时间：2025.06.16
    作者：不去幼儿园
"""
import numpy as np
import matplotlib.pyplot as plt
import random


class Node:
	def __init__(self, x, y):
		self.x = x
		self.y = y
		self.parent = None


def distance(n1, n2):
	return np.hypot(n1.x - n2.x, n1.y - n2.y)


def get_random_node(goal_sample_rate, goal):
	if random.random() < goal_sample_rate:
		return Node(goal.x, goal.y)
	return Node(random.uniform(0, 100), random.uniform(0, 100))


def steer(from_node, to_node, extend_length=5.0):
	dist = distance(from_node, to_node)
	theta = np.arctan2(to_node.y - from_node.y, to_node.x - from_node.x)
	new_x = from_node.x + extend_length * np.cos(theta)
	new_y = from_node.y + extend_length * np.sin(theta)
	new_node = Node(new_x, new_y)
	new_node.parent = from_node
	return new_node


def is_collision(node):
	# 简化处理：假设无障碍物
	return False


def rrt(start, goal, max_iter=500, goal_sample_rate=0.05):
	nodes = [start]
	for _ in range(max_iter):
		rnd = get_random_node(goal_sample_rate, goal)
		nearest = min(nodes, key=lambda n: distance(n, rnd))
		new_node = steer(nearest, rnd)

		if not is_collision(new_node):
			nodes.append(new_node)
			if distance(new_node, goal) < 5.0:
				goal.parent = new_node
				nodes.append(goal)
				break
	return nodes


def draw_path(last_node):
	path = []
	node = last_node
	while node:
		path.append((node.x, node.y))
		node = node.parent
	path = path[::-1]
	plt.plot([x for x, y in path], [y for x, y in path], '-r')


def draw_tree(nodes):
	for node in nodes:
		if node.parent:
			plt.plot([node.x, node.parent.x], [node.y, node.parent.y], '-g')


start = Node(10, 10)
goal = Node(90, 90)

nodes = rrt(start, goal)
draw_tree(nodes)
draw_path(goal)
plt.plot(start.x, start.y, "bs", label="Start")
plt.plot(goal.x, goal.y, "gs", label="Goal")
plt.legend()
plt.grid(True)
plt.axis([0, 100, 0, 100])
plt.title("RRT Path Planning (No Obstacles)")
plt.show()